#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include "MoLocalSdkInterface.h"
#include "MoImageRectify.h"
#include <sensor_msgs/Imu.h>

using namespace std;
using namespace cv;

#define MO_STREAM_TYPE_STEREO_RGBD        0x0010  //双目深度图敏感器输出的 视差图+参考图组成的混合图
#define MO_STREAM_TYPE_STEREO_ORIGIN_RGBD 0x0080  //双目深度图敏感器输出的 原始视差图+参考图组成的混合图

unsigned char SN[MO_MAX_SN_LEN];
int SN_Size;

struct MoStereoCameraIntrinsics
{
    uint16_t MagicCode;
    uint8_t Reserved[2];
    MoFullStereoCameraParams FullStereoCamParam;
};

int ShowRGBImage(int Handle, MoFrame *pFrm);
int SpliteImage(MoFrame *pInFrm, MoFrame *pOutFrm1, MoFrame *pOutFrm2, bool IfByHori);
int isp_image_spliteImage(MoFrame *pFrm, Mat matRGB_L, Mat matRGB_R);
void printf_camera_MoBinocularCameraParams(MoBinocularCameraParams pIspParams);

float ROIDispFillRate;

static unsigned char DispImgColorTable[3*8192];

#define II_RAW_DISPARITY_VAL_BIT ((unsigned short)0x1fff)

static void getDispImageColorTable()
{
    int i;

    DispImgColorTable[0] = 0;
    DispImgColorTable[1] = 0;
    DispImgColorTable[2] = 0;

    for (i = 1; i <= 24; i ++) 
    {
        DispImgColorTable[i * 3] = 255;
        DispImgColorTable[i * 3 + 1] = 255;
        DispImgColorTable[i * 3 + 2] = 255;
    }
    for (i = 25; i <= 40; i ++) 
    {
        DispImgColorTable[i * 3] = (int)(255 - ((255.0 - 128.0) / (40.0 - 24.0)) * (i - 24));
        DispImgColorTable[i * 3 + 1] = (int)(255 - ((255.0 - 128.0) / (40.0 - 24.0)) * (i - 24));
        DispImgColorTable[i * 3 + 2] = (int)(255 - ((255.0 - 128.0) / (40.0 - 24.0)) * (i - 24));
    }
    for (i = 41; i <= 64; i ++) 
    {
        DispImgColorTable[i * 3] = (int)(128 + (int)(5.291668 * (i - 41)));
        DispImgColorTable[i * 3 + 1] = (int)(128 - (int)(5.291668 * (i - 41)));
        DispImgColorTable[i * 3 + 2] = (int)(128 + (int)(5.291668 * (i - 41)));
    }
    for (i = 65; i <= 120; i ++) 
    {
        DispImgColorTable[i * 3] = (int)(255 - (int)(4.553571 * (i - 64)));
        DispImgColorTable[i * 3 + 1] = 0;
        DispImgColorTable[i * 3 + 2] = 255;
    }
    for (i = 121; i <= 176; i ++) 
    {
        DispImgColorTable[i * 3] = 0;
        DispImgColorTable[i * 3 + 1] = (int)(0 + (int)(4.553571 * (i - 120)));
        DispImgColorTable[i * 3 + 2] = 255;
    }
    for (i = 177; i <= 320; i++) {
        DispImgColorTable[i * 3] = 0;
        DispImgColorTable[i * 3 + 1] = 255;
        DispImgColorTable[i * 3 + 2] = (int)(255 - (int)(1.770833 * (i - 176)));
    }
    for (i = 321; i <= 800; i ++) 
    {
        DispImgColorTable[i * 3] = (int)(0 + (int)(0.53125 * (i - 320)));
        DispImgColorTable[i * 3 + 1] = 255;
        DispImgColorTable[i * 3 + 2] = 0;
    }
    for (i = 801; (i <= 2048); i ++) 
    {
        DispImgColorTable[i * 3] = 255;
        DispImgColorTable[i * 3 + 1] = (int)(255 - (int)(0.204327 * (i - 800)));
        DispImgColorTable[i * 3 + 2] = 0;
    }
    for (i = 2049; i < 8192; i ++) 
    {
        DispImgColorTable[i * 3] = 255;
        DispImgColorTable[i * 3 + 1] = 0;
        DispImgColorTable[i * 3 + 2] = 0;
    }
}
void moDrawDspImage(cv::Mat &debugImg, unsigned short *pDspImageData) {
    int x, y;
    unsigned char *pColorTable = DispImgColorTable;

    for (y = 0; y < debugImg.rows; y++) {
        uchar *pImageData = debugImg.data + y * debugImg.step;

        for (x = 0; x < debugImg.cols; x++) {
            int d = (*(pDspImageData++)) & II_RAW_DISPARITY_VAL_BIT;

            if (d > 0) {
                *(pImageData++) = pColorTable[d * 3 + 2];
                *(pImageData++) = pColorTable[d * 3 + 1];
                *(pImageData++) = pColorTable[d * 3];
            } else {
                *(pImageData++) = 0;
                *(pImageData++) = 0;
                *(pImageData++) = 0;
            }
        }
    }

    for (y = 0; y < 2800; y += 4)
        cv::line(debugImg, cv::Point(debugImg.cols - 20, y / 4),
                 cv::Point(debugImg.cols - 1, y / 4),
                 CV_RGB(pColorTable[y * 3], pColorTable[y * 3 + 1],
                        pColorTable[y * 3 + 2]),
                 1, 8, 0);

    
    return;
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "mo_s315_rgbd");

    ros::NodeHandle nh;

    // roslaunch params
    std::string camPath;
    int fps_;
    int PCLOUD_REDUCE_RATE;

    nh.param<int>("fps",fps_,15);
    nh.param<int>("point_cloud_down_sample",PCLOUD_REDUCE_RATE, 1);
    image_transport::ImageTransport it(nh);
    image_transport::Publisher img_pub_ispL = it.advertise("S315/image_isp_L", 2);
    image_transport::Publisher img_pub_ispR = it.advertise("S315/image_isp_R", 2);
    image_transport::Publisher dsp_depth_pub = it.advertise("S315/image_dsp_depth",2);
    image_transport::Publisher color_depth_pub = it.advertise("S315/image_color_depth",2);
    //image_transport::Publisher img_pub_simor_rgb = it.advertise("S315/image_simor_rgb", 2);
    ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 128);

    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_output", 2);
    pcl::PointCloud<pcl::PointXYZRGBA> cloud;
    sensor_msgs::PointCloud2 output;
    std_msgs::Header header;

    /* sdk version */
    char SdkVerion[16];
    int Ret = moLocalGetSdkVersion(SdkVerion);
    if(MO_REV_OK == Ret) printf("[OK]: SDK version is %s\n", SdkVerion);

    /* sdk open camera */
    int Handle = moLocalOpenCamera();
    if(MO_ILLEGAL_HANDLE == Handle)
    {
        cout << "[Error] Open camera failed! err = " << errno << endl; 
    }
    else 
        cout << "[OK] open camera successful!" << endl;      
    

    /* get isp rgb paranm */
    MoBinocularCameraParams pIspParams;
    Ret = moLocalGetIspBinocularParameter(&pIspParams);
    if(MO_REV_OK == Ret)
    {
        printf("[OK]: moLocalGetIspBinocularParameter OK \n");
        printf_camera_MoBinocularCameraParams(pIspParams);
    }
    else
    {
        printf("[Error]: moLocalGetIspBinocularParameter fail \n");
        return -1;
    } 

    /* sdk get camera Base and BXF */
    double Base, BxF;
    Ret = moLocalGetBaseAndBxf(&Base, &BxF);
    cout << "[OK] Base:" << Base << " Simor BXF:" << BxF << endl;

    /* sdk set camera mode RGBD */ 
    unsigned int set_stream = MO_STREAM_TYPE_STEREO_ORIGIN_RGBD; // origin dsp
    if( MO_REV_OK == moLocalSetStreamType(set_stream, 0))
    {
        printf("[OK] moLocalSetStreamType \n");
    }
    else
    {
        printf("[Error] moLocalSetStreamType fail");
        return -1;
    } 
    
    /* sdk cheak camera mode */
    unsigned int pStreamType;
    if( MO_REV_OK == moLocalGetStreamType(&pStreamType, 0))
    {
        printf("[OK] moLocalGetStreamType= 0x%X\n", pStreamType );
    }
    if(pStreamType != set_stream)
    {
        cout << "[error]: moLocalGetStreamType != moLocalSetStreamType " << endl;
        return -1;
    }

    /* sdk set camera fps (10----25) */
    Ret = moLocalSetFPS(fps_, 0);
    if(MO_REV_OK == Ret) printf("[OK]: moLocalSetFPS = %d \n", fps_ );
    else
    {
        printf("[Error]: moLocalSetFPS = %d error", fps_ );
        return -1;
    }

    /* sdk set imu */
    Ret = moLocalSetIMUEnable(true);
    if(MO_REV_OK == Ret) printf("[OK]: moLocalSetIMUEnable = true \n");
    else
    {
        printf("[Error]: moLocalLocalSetIMUEnable error" );
        return -1;
    }

    bool imu_pEnable;
    Ret = moLocalGetIMUEnable(&imu_pEnable);
    if(MO_REV_OK == Ret) printf("[OK]: moLocalGetIMUEnable = %d \n",imu_pEnable);
    else
    {
        printf("[Error]: moLocalGetIMUEnable error" );
        return -1;
    }

    MoFrame RGBDFrm, DispFrm, YuvFrm;
    MoFrame RGBFrm;
    
    // generate tables
    float* m_mapDspToDisZ_M  = new float[8192];
    float* m_mapDspToDisXY_M = new float[8192];
    for(int i=1; i<8192; i++){
        m_mapDspToDisZ_M[i]  = BxF * 32.0 / i / 1000.0;  // unit : m
        m_mapDspToDisXY_M[i] = Base *32.0 / i / 1000.0;
    }

    MoFrame *ppDispForVSLAM = NULL;
    MoIMUData imuData[MO_MAX_IMU_CNT_IN_QUEUE]; // imu max len 128

    ros::Rate loop_rate(300); // 3588 imu 200hz

    while (ros::ok())
    {
        ros::Time now = ros::Time::now();
        
        /****************************  [ imu ] **************************************/ 
    
        int imu_Count = 0;
        if(  imu_pEnable && MO_REV_OK == moLocalGetIMUData(imuData, &imu_Count))
        { 
            cout << "imu_Count = " << imu_Count << endl;
            for(int i=0; i<imu_Count; i++)
            {
                cout << " =============================== " << endl;
                cout << "imu.Timestamp = " << imuData[i].Timestamp << endl;
                cout << "imu.LinearAccelerationX = " << imuData[i].LinearAccelerationX << endl;
                cout << "imu.LinearAccelerationY = " << imuData[i].LinearAccelerationY << endl;
                cout << "imu.LinearAccelerationZ = " << imuData[i].LinearAccelerationZ << endl;
                cout << "imu.AngularVelocityX = " << imuData[i].AngularVelocityX << endl;
                cout << "imu.AngularVelocityY = " << imuData[i].AngularVelocityY << endl;
                cout << "imu.AngularVelocityZ = " << imuData[i].AngularVelocityZ << endl;

                sensor_msgs::Imu imu_info;
                imu_info.header.frame_id = "camera_frame";
                imu_info.header.stamp.fromSec(imuData[i].Timestamp * 1e-3);
                imu_info.linear_acceleration.x = imuData[i].LinearAccelerationX;
                imu_info.linear_acceleration.y = imuData[i].LinearAccelerationY;
                imu_info.linear_acceleration.z = imuData[i].LinearAccelerationZ;
                imu_info.angular_velocity.x = imuData[i].AngularVelocityX;
                imu_info.angular_velocity.y = imuData[i].AngularVelocityY;
                imu_info.angular_velocity.z = imuData[i].AngularVelocityZ;

                imu_pub.publish(imu_info);
            }
        }

        /****************************  [ RGB-D ]  **************************************/ 

        if(MO_REV_OK == (Ret = moLocalGetOneRGBDFrame(&RGBDFrm))) 
        {
            if(MO_REV_OK == (Ret = moLocalDecodeRGBDFrame(&RGBDFrm, &DispFrm, &YuvFrm)))
            {
                /* sdk rgbd_image noise reduction */
                moLocalEnhanceRemoveDupTexture(&ppDispForVSLAM,&DispFrm,&YuvFrm,15000,0,0,640,360, &ROIDispFillRate); // 15m   (0,0)--(640,360) 全图
                /* sdk remove noise */
                moLocalOutlierRemove(&DispFrm);
                
             
                /* sdk get color_depth */
                Mat mat_color_depth(DispFrm.ResolutionHeight, DispFrm.ResolutionWidth, CV_8UC3);
                // moApplyColorToDisparityImage(mat_color_depth.step, &DispFrm, mat_color_depth.data);
                getDispImageColorTable();
                moDrawDspImage(mat_color_depth, (uint16_t *)DispFrm.Data);
                Mat mat_dps_depth(DispFrm.ResolutionHeight, DispFrm.ResolutionWidth, CV_16UC1, DispFrm.Data);  // U16

                double image_timestamp = RGBDFrm.TimeStamp * 1e-3;
                header.stamp.fromSec(image_timestamp);
                header.frame_id = "camera_frame";
                //cout << "RGBD timestamp = " << image_timestamp << " ms" <<endl;
                
                // ros pubilsh color_depth image
                cv_bridge::CvImage cv_color_depth = cv_bridge::CvImage(header, "bgr8", mat_color_depth);
                color_depth_pub.publish(cv_color_depth.toImageMsg());

                // ros pubilsh dsp_depth image u16
                cv_bridge::CvImage cv_dsp_depth = cv_bridge::CvImage(header, "mono16", mat_dps_depth);
                dsp_depth_pub.publish(cv_dsp_depth.toImageMsg());

                //simor-RGB, this no used
                // Mat matRGB;
                // Mat matYUV420( YuvFrm.ResolutionHeight*3/2, YuvFrm.ResolutionWidth, CV_8UC1, YuvFrm.Data);
                // cvtColor(matYUV420, matRGB, COLOR_YUV2BGR_I420);
                // //imshow("simor-RGB",matRGB);
                // cv_bridge::CvImage cv_image_simor_rgb = cv_bridge::CvImage(header, "bgr8", matRGB);
                // img_pub_simor_rgb.publish(cv_image_simor_rgb.toImageMsg());


                // cloud-point
                int nHeight = DispFrm.ResolutionHeight;
                int nWidth = DispFrm.ResolutionWidth;
                int nHeight_half = DispFrm.ResolutionHeight/2;  // cx
                int nWidth_half = DispFrm.ResolutionWidth/2;    // cy
    
                unsigned short* ptr_dis = (unsigned short* )DispFrm.Data; // get depth data (u16)
                // statistical effective cloud point number
                int nCount=0 ,total_count= 0;
                for(int y = 0; y<nHeight; y++)
                {
                    for(int x = 0; x<nWidth; x++, ptr_dis++)
                    {
                        if(*ptr_dis > 0)  total_count++;
                    }
                }

                // resize cloud points
                cloud.clear();
                cloud.width = total_count;
                cloud.height = 1;
                cloud.points.resize(cloud.width * cloud.height);
                printf("total cloud number : %d \n",total_count);

                // fill cloud point data
                float max_x=0,max_y=0,max_z=0;
                ptr_dis = (unsigned short* )DispFrm.Data;

                for(int y = 0; y<nHeight; y++)
                {
                    for(int x = 0; x<nWidth; x++)
                    {
                        unsigned short usd = *ptr_dis;
                        if( usd > 0 )
                        {
                            cloud.points[nCount].z = m_mapDspToDisZ_M[usd];
                            cloud.points[nCount].x = m_mapDspToDisXY_M[usd]*(nWidth_half-x);   // this should use cx cy
                            cloud.points[nCount].y = m_mapDspToDisXY_M[usd]*(nHeight_half-y);
                            cloud.points[nCount].a = 255;
                            nCount ++;
                            
                        }
                        ptr_dis++;
                    }
                }  
                
                moLocalReleaseFrame(&DispFrm);  // release the decoded disparity frame and yub frame
                moLocalReleaseFrame(&YuvFrm);
            }

            moLocalReleaseFrame(&RGBDFrm);  // release RGBD frame;

            // ros publish
            pcl::toROSMsg(cloud, output);
            output.header.frame_id   = "camera_frame";
            output.header.stamp = now;
            pcl_pub.publish(output);

        }

        /**************************** RGB (right + left) *************************/ 

        if(MO_REV_OK == (Ret = moLocalGetOneRGBFrame(&RGBFrm)))
        {
            MoFrame isp_left_frm, isp_right_frm;

            Mat matRGB_L = Mat(480,640,CV_8UC3);
            Mat matRGB_R = Mat(480,640,CV_8UC3);

            if (MO_REV_OK == moLocalSplitRGBFrame(&RGBFrm, &isp_left_frm, &isp_right_frm)) 
            {
                // left
                Mat matYuv422_L = Mat(isp_left_frm.ResolutionHeight, isp_left_frm.ResolutionWidth, CV_8UC2, isp_left_frm.Data);
                cv::cvtColor(matYuv422_L, matRGB_L, cv::COLOR_YUV2BGR_YUYV);
                
                // right
                Mat matYuv422_R = Mat(isp_right_frm.ResolutionHeight, isp_right_frm.ResolutionWidth, CV_8UC2, isp_right_frm.Data);
                cv::cvtColor(matYuv422_R, matRGB_R, cv::COLOR_YUV2BGR_YUYV);

                //ros pubilsh
                double image_timestamp = RGBFrm.TimeStamp * 1e-3;
                header.stamp.fromSec(image_timestamp);
                header.frame_id = "camera_frame";
                //cout << "RGB timestamp = " << image_timestamp << endl;
            
                cv_bridge::CvImage cv_image_ispL = cv_bridge::CvImage(header, "bgr8", matRGB_L);
                cv_bridge::CvImage cv_image_ispR = cv_bridge::CvImage(header, "bgr8", matRGB_R);
                img_pub_ispL.publish(cv_image_ispL.toImageMsg());
                img_pub_ispR.publish(cv_image_ispR.toImageMsg());
            }


            moLocalReleaseFrame(&isp_left_frm);
            moLocalReleaseFrame(&isp_right_frm);
            moLocalReleaseFrame(&RGBFrm);  
        }

        
        ros::spinOnce();
        loop_rate.sleep();
    }

    free(m_mapDspToDisZ_M);
    free(m_mapDspToDisXY_M);
    moLocalCloseCamera();
    
    return 0;
}

void printf_camera_MoBinocularCameraParams(MoBinocularCameraParams pIspParams)
{
    // typedef struct _mo_mono_camera_params 
    // {
    //     float fx;
    //     float fy;
    //     float cx;
    //     float cy;
    //     float radial_distortion_k[3];
    //     float tangential_distortion_p[2];
    // } MoMonoCameraParams;

    // typedef struct _mo_binocular_camera_params 
    // {
    //     MoMonoCameraParams leftCamera;
    //     MoMonoCameraParams rightCamera;
    //     float rotation_r[3];
    //     float translation_t[3];
    //     uint16_t resolution_width;
    //     uint16_t resolution_height;
    // } MoBinocularCameraParams;
    cout << "=========== MoMonoCameraParams =============" << endl;
    cout << "[ isp left ]" << endl;
    // left
    cout << " fx: " << pIspParams.leftCamera.fx << endl;
    cout << " fy: " << pIspParams.leftCamera.fy << endl;
    cout << " cx: " << pIspParams.leftCamera.cx << endl;
    cout << " cy: " << pIspParams.leftCamera.cy << endl;
    // k1 k2 k3
    cout << " k1: " << pIspParams.leftCamera.radial_distortion_k[0] << endl;
    cout << " k2: " << pIspParams.leftCamera.radial_distortion_k[1] << endl;
    cout << " k3: " << pIspParams.leftCamera.radial_distortion_k[2] << endl;
    // p1 p2 p3
    cout << " p1: " << pIspParams.leftCamera.tangential_distortion_p[0] << endl;
    cout << " p2: " << pIspParams.leftCamera.tangential_distortion_p[1] << endl;
    cout << " p3: " << pIspParams.leftCamera.tangential_distortion_p[2] << endl;

    cout << "[ isp right ]" << endl;
    // right 
    cout << " fx: " << pIspParams.rightCamera.fx << endl;
    cout << " fy: " << pIspParams.rightCamera.fy << endl;
    cout << " cx: " << pIspParams.rightCamera.cx << endl;
    cout << " cy: " << pIspParams.rightCamera.cy << endl;
    // k1 k2 k3
    cout << " k1: " << pIspParams.rightCamera.radial_distortion_k[0] << endl;
    cout << " k2: " << pIspParams.rightCamera.radial_distortion_k[1] << endl;
    cout << " k3: " << pIspParams.rightCamera.radial_distortion_k[2] << endl;
    // p1 p2 p3
    cout << " p1: " << pIspParams.rightCamera.tangential_distortion_p[0] << endl;
    cout << " p2: " << pIspParams.rightCamera.tangential_distortion_p[1] << endl;
    cout << " p3: " << pIspParams.rightCamera.tangential_distortion_p[2] << endl;

    cout << "[ R T ]" << endl;
    cout << " r1: " << pIspParams.rotation_r[0] << endl;
    cout << " r2: " << pIspParams.rotation_r[1] << endl;
    cout << " r3: " << pIspParams.rotation_r[2] << endl;

    cout << " t1: " << pIspParams.translation_t[0] << endl;
    cout << " t2: " << pIspParams.translation_t[1] << endl;
    cout << " t3: " << pIspParams.translation_t[2] << endl;

    cout << " resolution_width:  " << pIspParams.resolution_width << endl;
    cout << " resolution_height: " << pIspParams.resolution_height << endl;
    cout << "===========================================" << endl;
}


// function: isp-image(1280*480) to 2*RGB-image(640*480)
int isp_image_spliteImage(MoFrame *pFrm, Mat matRGB_1, Mat matRGB_2)
{
    MoFrame pOutFrm1;
    MoFrame pOutFrm2;

    Mat matYuv422_1;
    Mat matYuv420_1;
    Mat matRgb888_1;

    Mat matYuv422_2;
    Mat matYuv420_2;
    Mat matRgb888_2;

    //X3:YUV420. RK3588:YUV422
    SpliteImage(pFrm, &pOutFrm1, &pOutFrm2, true);

    if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_RGB888)
    {
        matRgb888_1 = Mat(pOutFrm1.ResolutionHeight, pOutFrm1.ResolutionWidth, CV_8UC3, pOutFrm1.Data);
        matRgb888_2 = Mat(pOutFrm2.ResolutionHeight, pOutFrm2.ResolutionWidth, CV_8UC3, pOutFrm2.Data);
        //cout << "[debug] isp RGB888" << endl;
    }
    else 
    if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV420_NV12)
    {
        matYuv420_1 = Mat(pOutFrm1.ResolutionHeight*3/2, pOutFrm1.ResolutionWidth, CV_8UC1, pOutFrm1.Data);
        matYuv420_2 = Mat(pOutFrm2.ResolutionHeight*3/2, pOutFrm2.ResolutionWidth, CV_8UC1, pOutFrm2.Data);
        //cout << "[debug] isp YUV420_NV12" << endl;
    }
    else
    {
        matYuv422_1 = Mat(pOutFrm1.ResolutionHeight, pOutFrm1.ResolutionWidth, CV_8UC2, pOutFrm1.Data); 
        matYuv422_2 = Mat(pOutFrm2.ResolutionHeight, pOutFrm2.ResolutionWidth, CV_8UC2, pOutFrm2.Data); 
    }

    //cout << "[debug] RGB_1 resolution "<< pOutFrm1.ResolutionWidth <<  " * " <<pOutFrm1.ResolutionHeight << endl;
    //cout << "[debug] RGB_2 resolution "<< pOutFrm2.ResolutionWidth <<  " * " <<pOutFrm2.ResolutionHeight << endl;

    // transform RGB show
    // x3
    if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV420_NV12)
    {
        cvtColor(matYuv420_1, matRGB_1, COLOR_YUV2RGB_NV12);
        cvtColor(matYuv420_2, matRGB_2, COLOR_YUV2RGB_NV12);
        //cout << "[debug] COLOR_YUV2RGB_NV12" << endl;

        // resizeWindow(RGB_FRM_WIN_NAME, RightFrm.ResolutionWidth, RightFrm.ResolutionHeight);
        resizeWindow("RGB-1", pOutFrm1.ResolutionWidth, pOutFrm1.ResolutionHeight);
        resizeWindow("RGB-2", pOutFrm2.ResolutionWidth, pOutFrm2.ResolutionHeight);
        //imshow("RGB-1", matRGB_1);
        //imshow("RGB-2", matRGB_2);
        moLocalReleaseFrame(&pOutFrm1); 
        moLocalReleaseFrame(&pOutFrm2); 
    }
    // RK3588
    else if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV422_UYVY)
    {

        cvtColor(matYuv422_1, matRGB_1, COLOR_YUV2RGB_UYVY);
        cvtColor(matYuv422_2, matRGB_2, COLOR_YUV2RGB_UYVY);

        resizeWindow("RGB-1", pOutFrm1.ResolutionWidth, pOutFrm1.ResolutionHeight);
        resizeWindow("RGB-2", pOutFrm2.ResolutionWidth, pOutFrm2.ResolutionHeight);
        //imshow("RGB-1", matRGB_1);
        //imshow("RGB-2", matRGB_2);
        moLocalReleaseFrame(&pOutFrm1); 
        moLocalReleaseFrame(&pOutFrm2); 

    }
    else
    {
        cout << "===== [debug] type error =====" << endl;
        return -1;
    } 
    return 0;
}



int ShowRGBImage(int Handle, MoFrame *pFrm, Mat &out_matRGB_1, Mat &out_matRGB_2)
{
    // set image splite
    // true:  640*480 * 2
    // flase: 1280*480 * 1
    bool spliteImage = true;  

    if( spliteImage )
    {
        MoFrame pOutFrm1;
        MoFrame pOutFrm2;

        Mat matYuv422_1;
        Mat matYuv420_1;
        Mat matRgb888_1;

        Mat matYuv422_2;
        Mat matYuv420_2;
        Mat matRgb888_2;

        Mat matRGB_1;
        Mat matRGB_2;

        //X3:YUV420. RK3588:YUV422
        SpliteImage(pFrm, &pOutFrm1, &pOutFrm2, true);

        if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_RGB888)
        {
            matRgb888_1 = Mat(pOutFrm1.ResolutionHeight, pOutFrm1.ResolutionWidth, CV_8UC3, pOutFrm1.Data);
            matRgb888_2 = Mat(pOutFrm2.ResolutionHeight, pOutFrm2.ResolutionWidth, CV_8UC3, pOutFrm2.Data);
            cout << "[debug] isp RGB888" << endl;
        }
        else 
        if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV420_NV12)
        {
            matYuv420_1 = Mat(pOutFrm1.ResolutionHeight*3/2, pOutFrm1.ResolutionWidth, CV_8UC1, pOutFrm1.Data);
            matYuv420_2 = Mat(pOutFrm2.ResolutionHeight*3/2, pOutFrm2.ResolutionWidth, CV_8UC1, pOutFrm2.Data);
            cout << "[debug] isp YUV420_NV12" << endl;
        }
        else
        {
            matYuv422_1 = Mat(pOutFrm1.ResolutionHeight, pOutFrm1.ResolutionWidth, CV_8UC2, pOutFrm1.Data); 
            matYuv422_2 = Mat(pOutFrm2.ResolutionHeight, pOutFrm2.ResolutionWidth, CV_8UC2, pOutFrm2.Data); 
        }

        cout << "[debug] RGB_1 resolution "<< pOutFrm1.ResolutionWidth <<  " * " <<pOutFrm1.ResolutionHeight << endl;
        cout << "[debug] RGB_2 resolution "<< pOutFrm2.ResolutionWidth <<  " * " <<pOutFrm2.ResolutionHeight << endl;

        // transform RGB show
        // x3
        if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV420_NV12)
        {
            cvtColor(matYuv420_1, matRGB_1, COLOR_YUV2RGB_NV12);
            cvtColor(matYuv420_2, matRGB_2, COLOR_YUV2RGB_NV12);
            cout << "[debug] COLOR_YUV2RGB_NV12" << endl;

            // resizeWindow(RGB_FRM_WIN_NAME, RightFrm.ResolutionWidth, RightFrm.ResolutionHeight);
            // resizeWindow("RGB-1", pOutFrm1.ResolutionWidth, pOutFrm1.ResolutionHeight);
            // resizeWindow("RGB-2", pOutFrm2.ResolutionWidth, pOutFrm2.ResolutionHeight);
            // imshow("RGB-1", matRGB_1);
            // imshow("RGB-2", matRGB_2);

            out_matRGB_1 = matRGB_1.clone();
            out_matRGB_2 = matRGB_2.clone();

            moLocalReleaseFrame(&pOutFrm1); 
            moLocalReleaseFrame(&pOutFrm2); 
        }
        // RK3588
        else if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV422_UYVY)
        {

            cvtColor(matYuv422_1, matRGB_1, COLOR_YUV2RGB_UYVY);
            cvtColor(matYuv422_2, matRGB_2, COLOR_YUV2RGB_UYVY);

            // resizeWindow("RGB-1", pOutFrm1.ResolutionWidth, pOutFrm1.ResolutionHeight);
            // resizeWindow("RGB-2", pOutFrm2.ResolutionWidth, pOutFrm2.ResolutionHeight);
            // imshow("RGB-1", matRGB_1);
            // imshow("RGB-2", matRGB_2);

            out_matRGB_1 = matRGB_1.clone();
            out_matRGB_2 = matRGB_2.clone();

            moLocalReleaseFrame(&pOutFrm1); 
            moLocalReleaseFrame(&pOutFrm2); 

        }
        else
        {
            cout << "=====[debug] type error=====" << endl;
            return MO_REV_DEV_DATA_WRONG;
        } 
    }
    // no Splite
    else
    {
        Mat matRGB;
        Mat matYuv422;
        Mat matYuv420;
        Mat matRgb888;

        if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_RGB888)
            matRgb888 = Mat(pFrm->ResolutionHeight, pFrm->ResolutionWidth, CV_8UC3, pFrm->Data);
        else 
        if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV420_NV12)
            matYuv420 = Mat(pFrm->ResolutionHeight*3/2, pFrm->ResolutionWidth, CV_8UC1, pFrm->Data);
            // matYuv420 = Mat(pFrm->ResolutionHeight, pFrm->ResolutionWidth, CV_8UC1, pFrm->Data);
        else
            matYuv422 = Mat(pFrm->ResolutionHeight, pFrm->ResolutionWidth, CV_8UC2, pFrm->Data); 
        

        // RK3588
        if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV422_YVYU)
        {
            cvtColor(matYuv422, matRGB, COLOR_YUV2RGB_YUYV);
            cout << "[debug] COLOR_YUV2RGB_YUYV" << endl;
        } 
        else if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV422_YUYV)
        {
            cvtColor(matYuv422, matRGB, COLOR_YUV2RGB_YUYV);
            cout << "[debug] COLOR_YUV2RGB_YUYV" << endl;
        } 
        else if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV422_UYVY) 
        {
            cvtColor(matYuv422, matRGB, COLOR_YUV2RGB_UYVY);
            cout << "[debug] COLOR_YUV2RGB_UYVY" << endl;
        }
        else if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_RGB888) 
        {
            cvtColor(matRgb888, matRGB, COLOR_RGB2BGR);
            cout << "[debug] COLOR_RGB2BGR" << endl;
        }
        // X3
        else if(pFrm->RGBImageType == MO_RGB_IMAGE_TYPE_YUV420_NV12)
        {
            cvtColor(matYuv420, matRGB, COLOR_YUV2RGB_NV12);
            cout << "[debug] COLOR_YUV2RGB_NV12" << endl;
        }
        else
        {
            cout << "=====[debug] type error=====" << endl;
            return MO_REV_DEV_DATA_WRONG;
        } 

        // resizeWindow("isp-RGB_1280*480", pFrm->ResolutionWidth, pFrm->ResolutionHeight);
        // imshow("isp-RGB_1280*480", matRGB);
    }
    
    return MO_REV_OK;
}


int __SpilteNormalImage(MoFrame *pInFrm, MoFrame *pOutFrm1, MoFrame *pOutFrm2, bool IfByHori)
{
    uint32_t OutFrmSize = pInFrm->ResolutionWidth*pInFrm->ResolutionHeight*pInFrm->BitsPerPixel/16;

    if(NULL != pOutFrm1) 
    {
        memcpy(pOutFrm1, pInFrm, sizeof(MoFrame));
        if(IfByHori) pOutFrm1->ResolutionWidth /= 2;
        else pOutFrm1->ResolutionHeight /=2;
        pOutFrm1->Data = malloc(OutFrmSize);
        if(NULL == pOutFrm1->Data) return MO_REV_BUF_ALLOC_FAIL;
    }

    if(NULL != pOutFrm2) 
    {
        memcpy(pOutFrm2, pInFrm, sizeof(MoFrame));
        if(IfByHori) pOutFrm2->ResolutionWidth /= 2;
        else pOutFrm2->ResolutionHeight /=2;
        pOutFrm2->Data = malloc(OutFrmSize);
        if(NULL == pOutFrm2->Data)
        {
            if(NULL != pOutFrm1) free(pOutFrm1->Data);
            return MO_REV_BUF_ALLOC_FAIL;
        }
    }

    
    uint8_t *pSrc = (uint8_t *)pInFrm->Data;
    uint8_t *pDst1 = NULL, *pDst2 = NULL;
    
    if(NULL != pOutFrm1) pDst1 = (uint8_t *)pOutFrm1->Data;
    if(NULL != pOutFrm2) pDst2 = (uint8_t *)pOutFrm2->Data;


    if(IfByHori)
    {
        uint16_t Width = pInFrm->ResolutionWidth * pInFrm->BitsPerPixel/ 16;

        for(int i = 0; i < pInFrm->ResolutionHeight; i ++)
        {
            if(NULL != pOutFrm1) 
            {
                memcpy(pDst1, pSrc, Width);
                pDst1 += Width;
            }

            pSrc += Width;

            if(NULL != pOutFrm2)
            {
                memcpy(pDst2, pSrc, Width);
                pDst2 += Width;
            }

            pSrc += Width;
            
        }
    }
    else
    {
        if(NULL != pOutFrm1) memcpy(pDst1, pSrc, OutFrmSize);
        pSrc += OutFrmSize;
        if(NULL != pOutFrm2) memcpy(pDst2, pSrc, OutFrmSize);
    }

    return MO_REV_OK;
}

int __SpilteYuv420Image(MoFrame *pInFrm, MoFrame *pOutFrm1, MoFrame *pOutFrm2, bool IfByHori)
{
    uint16_t Width = pInFrm->ResolutionWidth / 2;
    uint16_t Height = pInFrm->ResolutionHeight;

    int i;
    uint8_t *pDst1 = NULL;
    uint8_t *pDst2 = NULL;

    if(NULL != pOutFrm1)
    {
        memcpy(pOutFrm1, pInFrm, sizeof(MoFrame));
        pOutFrm1->Data = malloc(Width*Height*3/2);
        pDst1 = (uint8_t *)pOutFrm1->Data;
        if(NULL == pDst1) return MO_REV_BUF_ALLOC_FAIL;
        if(IfByHori) pOutFrm1->ResolutionWidth = Width;
        else pOutFrm1->ResolutionHeight = Height / 2;
    }

    if(NULL != pOutFrm2)
    {
        memcpy(pOutFrm2, pInFrm, sizeof(MoFrame));
        pOutFrm2->Data = malloc(Width*Height*3/2);
        pDst2 = (uint8_t *)pOutFrm2->Data;
        if(NULL == pDst2) 
        {
            if(NULL != pDst1) free(pDst1);
            return MO_REV_BUF_ALLOC_FAIL;
        }
        if(IfByHori) pOutFrm2->ResolutionWidth = Width;
        else pOutFrm2->ResolutionHeight = Height / 2;
    }

    uint8_t *pSrc = (uint8_t *)pInFrm->Data;

    if(IfByHori)
    {
        Height *= 3;
        Height >>= 1;

        for(i = 0; i < Height; i ++)
        {
            if(NULL != pDst1) 
            {
                memcpy(pDst1, pSrc, Width);
                pDst1 += Width;
            }

            pSrc += Width;

            if(NULL != pDst2)
            {
                memcpy(pDst2, pSrc, Width);
                pDst2 += Width;
            }

            pSrc += Width;
        }
    }
    else
    {
        uint32_t FrmSize = Height*Width;

        if(NULL != pDst1)
        {
            memcpy(pDst1, pSrc, FrmSize);
            pDst1 += FrmSize;
        };

        pSrc += FrmSize;

        if(NULL != pDst2)
        {
            memcpy(pDst2, pSrc, FrmSize);
            pDst2 += FrmSize;
        };

        pSrc += FrmSize;

        FrmSize >>= 1;

        if(NULL != pDst1)
        {
            memcpy(pDst1, pSrc, FrmSize);
            pDst1 += FrmSize;
        };

        pSrc += FrmSize;

        if(NULL != pDst2)
        {
            memcpy(pDst2, pSrc, FrmSize);
            pDst2 += FrmSize;
        };

    }

    return MO_REV_OK;
}

int SpliteImage(MoFrame *pInFrm, MoFrame *pOutFrm1, MoFrame *pOutFrm2, bool IfByHori)
{
    if((NULL == pInFrm) || (NULL == pInFrm->Data)) return MO_REV_PARAM_ERR;
    if((NULL == pOutFrm1) && (NULL == pOutFrm2)) return MO_REV_OK;

    switch(pInFrm->RGBImageType)
    {
        case MO_RGB_IMAGE_TYPE_RGB888:
        case MO_RGB_IMAGE_TYPE_YUV422_YUYV:
        case MO_RGB_IMAGE_TYPE_YUV422_YVYU:
        case MO_RGB_IMAGE_TYPE_YUV422_UYVY:
            return __SpilteNormalImage(pInFrm, pOutFrm1, pOutFrm2, IfByHori);
        case MO_RGB_IMAGE_TYPE_YUV420_NV12:
        case MO_RGB_IMAGE_TYPE_YUV420_I420:
            return __SpilteYuv420Image(pInFrm, pOutFrm1, pOutFrm2, IfByHori);
    }

    return MO_REV_NOT_SUPPORT;
}




